#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <cmath>
#include <mutex>
#include <thread>
#include <chrono>
#include <boost/concept_check.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include<pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include<pcl/filters/voxel_grid.h>
#include<pcl/filters/passthrough.h>

#include <opencv2/opencv.hpp>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_broadcaster.h>
#include <cv_bridge/cv_bridge.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>

//#include <kinect2_bridge/kinect2_definitions.h>

#include "orbslam2_pointcloud/System.h"

using namespace std;

enum ORBSLAM_MODE
{
    Mono = 0,
    RGBD ,
    STEREO
};
		  
class Receiver
{
    public:
			enum Mode
			{
				NOTHING = 0,
				IMAGE ,
				DEPTH,
				BOTH,
				CLOUD
			};

    private:
			std::mutex lock;

			//const std::string topicColor, topicDepth,cameraParamFile , orbVocFile; 
			bool mbuseExact, mbuseCompressed;
			bool mborbshow;
			char mborbmode;

			Mode memode;
			bool mbupdateImage, mbupdateCloud; //code flage 
			bool mbview,mbsave,mbrunning,mbpublishPointCloud;
	 
                 
			const size_t queueSize;

			cv::Mat mcolor, mdepth;
			cv::Mat cameraMatrixColor, cameraMatrixDepth;
			cv::Mat lookupX, lookupY;

			typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactSyncPolicy;
			typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximateSyncPolicy;

			ros::NodeHandle nh;
			ros::AsyncSpinner spinner;
			ros::Publisher pub_pointcloud;
		  
			image_transport::ImageTransport it;
			image_transport::SubscriberFilter *subImageColor, *subImageDepth;
			message_filters::Subscriber<sensor_msgs::CameraInfo> *subCameraInfoColor, *subCameraInfoDepth;

			message_filters::Synchronizer<ExactSyncPolicy> *syncExact;
			message_filters::Synchronizer<ApproximateSyncPolicy> *syncApproximate;

			std::thread imageViewerThread;
			

			pcl::PointCloud<pcl::PointXYZRGBA> ::Ptr outputMap;
			pcl::PCDWriter writer;
			std::ostringstream oss;
			std::vector<int> params;
 
	//ORB-SLAM slam
                  ORB_SLAM2::System*orbslam = nullptr;

public:
        Receiver(const std::string &topicColor, const std::string &topicDepth,
			   const std::string &cameraParamFile,const std::string orbVocFile,
			   const char orbmode, const bool orbshow)
                      :queueSize(5),nh("~"), spinner(0), it(nh)
		{
		
			mbpublishPointCloud = false;
			memode =NOTHING;
			mbupdateImage = false;
			mbupdateCloud= false;
			mbsave= false;
			mbrunning= true;

			mbuseExact = false;  //
			mbuseCompressed = false;

			mborbmode =orbmode;
			mborbshow = orbshow;

			cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F); // 相机内参数矩阵
			cameraMatrixDepth = cv::Mat::zeros(3, 3, CV_64F);
					
				
			if(mborbmode == RGBD)
			{
				orbslam = new ORB_SLAM2::System(orbVocFile,cameraParamFile,ORB_SLAM2::System::RGBD,mborbshow);
				mbpublishPointCloud = true;
			}
			else if(mborbmode == Mono)
			{
				orbslam = new ORB_SLAM2::System(orbVocFile,cameraParamFile,ORB_SLAM2::System::MONOCULAR,mborbshow);
			}
			else if(mborbmode == STEREO)
			{
				orbslam = new ORB_SLAM2::System(orbVocFile,cameraParamFile,ORB_SLAM2::System::STEREO,mborbshow);
			}
			else
				;
					
			pub_pointcloud= nh.advertise<sensor_msgs::PointCloud2> ("PointCloudOutput", 10); //创建ROS的发布节点
				
		
			std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info";
			std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info";

			image_transport::TransportHints hints(mbuseCompressed ? "compressed" : "raw");
			subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints);
			subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints);
			subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize);
			subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize);

			cout<<"topicColor: "<<topicColor<<endl;
			cout<<"topicDepth: "<<topicDepth<<endl;
			cout<<"subCameraInfoColor: "<<topicCameraInfoColor<<endl;
			cout<<"subCameraInfoDepth: "<<topicCameraInfoDepth<<endl;
			cout<<"cameraParamFile: "<<cameraParamFile<<endl<<endl;

			if(mbuseExact)
			{
				syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
				syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
			}
			else
			{
				syncApproximate = new message_filters::Synchronizer<ApproximateSyncPolicy>(ApproximateSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
				syncApproximate->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
			}
		}

		~Receiver()
		{
			if(orbslam)
			{
			orbslam->Shutdown();
			delete orbslam;
			}
		}

		void run()
		{
		start();
		stop();
		}


private:
void start()
{
	spinner.start();

	std::chrono::milliseconds duration(100);
	while(!mbupdateImage || !mbupdateCloud)
	{
		if(!ros::ok())
		{
			return;
		}
		std::this_thread::sleep_for(duration);
		cout<<" wait publisher publish the topic .... "<<endl;
	}

	cv::Mat color, depth, depthDisp, combined; // local varibale
	
	std::chrono::time_point<std::chrono::high_resolution_clock> start, now; 
	double fps = 0;
	size_t frameCount = 0;
	std::ostringstream oss;
	
	const cv::Point pos(5, 15); //信息显示的位置
	const cv::Scalar colorText = CV_RGB(255, 255, 255);
	const double sizeText = 0.5;
	const int lineText = 1;
	const int font = cv::FONT_HERSHEY_SIMPLEX;
	
	cout << "starting..."<<endl;;
	
	start = std::chrono::high_resolution_clock::now();
	
	
	for(; mbrunning && ros::ok();)
	{	
		if(mbupdateImage)//数据发送到ORB-SLAM
		{
			lock.lock();
			color = this->mcolor;
			depth = this->mdepth;
			mbupdateImage = false;
			lock.unlock();
			if(orbslam)
			{
				if(mborbmode == RGBD)
				{
					orbslam->TrackRGBD(color,depth,ros::Time::now().toSec()); 
				}
				else if(mborbmode == Mono)
				{
					orbslam->TrackMonocular(color,ros::Time::now().toSec());
				}
				else if(mborbmode == STEREO)
				{
					orbslam->TrackStereo(color,depth,ros::Time::now().toSec());
				}
				else
				;	
			}
			
			++frameCount; //计算帧频率
			now = std::chrono::high_resolution_clock::now();
			double elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() / 1000.0;
			if(elapsed >= 1.0)
			{
				fps = frameCount / elapsed;
				oss.str("");
				oss << "fps: " << fps << " ( " << elapsed / frameCount * 1000.0 << " ms)";
				start = now;
				frameCount = 0;
			
			}
			
			if(mbpublishPointCloud)
			{
				orbslam->getPointCloudMap(outputMap);
				pointcloudpublish(*outputMap);
			}
			mbview  = true;
		}
		
		if(mbview ) //优先跟新ORB track 
		{
			mbview = false;
			if(memode == NOTHING )
			{
				;
			}
			else if(memode == IMAGE )
				{
				cv::putText(color, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
				cv::imshow("color image", color);
				cv::waitKey(1);
			}
			else if(memode == DEPTH )
			{
				dispDepth(color, depthDisp, 1); //maxValue = 1
				cv::putText(depthDisp, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
				cv::imshow("depth image",depthDisp);
			}
			else if(memode == BOTH )
			{
				combine(color, depth, combined);
				cv::putText(combined, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
				cv::imshow("combined image",combined);
			}
			else if(memode == CLOUD )
			{
				//generatePointCloud(depth, color, cloud);					
				//cloudViewer();
			}
		}
	}
}
void stop()
{
		cv::destroyAllWindows();  // endup the file 
		cv::waitKey(100);
		spinner.stop();

		if(mbuseExact)
		{
			delete syncExact;
		}
		else
		{
			delete syncApproximate;
		}

		delete subImageColor;
		delete subImageDepth;
		delete subCameraInfoColor;
		delete subCameraInfoDepth;

		mbrunning = false;
		if(memode == BOTH)
		{
			imageViewerThread.join();
		}
}

void callback(const sensor_msgs::Image::ConstPtr imageColor, const sensor_msgs::Image::ConstPtr imageDepth,
	      const sensor_msgs::CameraInfo::ConstPtr cameraInfoColor, const sensor_msgs::CameraInfo::ConstPtr cameraInfoDepth)
 {
	cv::Mat color, depth;

	//readCameraInfo(cameraInfoColor, cameraMatrixColor);
	//readCameraInfo(cameraInfoDepth, cameraMatrixDepth);
	//  readImage(imageColor, color);
	//   readImage(imageDepth, depth);
	cv_bridge::CvImageConstPtr pCvImage;

	pCvImage = cv_bridge::toCvShare(imageColor, "bgr8");
	pCvImage->image.copyTo(color);
	pCvImage = cv_bridge::toCvShare(imageDepth, imageDepth->encoding); //imageDepth->encoding
	pCvImage->image.copyTo(depth);
	// IR image input
	if(color.type() == CV_16U)
	{
		cv::Mat tmp;
		color.convertTo(tmp, CV_8U, 0.02);
		cv::cvtColor(tmp, color, CV_GRAY2BGR);
	}

	lock.lock();
	this->mcolor = color;
	this->mdepth = depth;
	mbupdateImage = true;
	mbupdateCloud = true;
	lock.unlock();
 }

void readImage(const sensor_msgs::Image::ConstPtr msgImage, cv::Mat &image) const
{
	cv_bridge::CvImageConstPtr pCvImage;
	pCvImage = cv_bridge::toCvShare(msgImage, msgImage->encoding);
	pCvImage->image.copyTo(image);
}

void readCameraInfo(const sensor_msgs::CameraInfo::ConstPtr cameraInfo, cv::Mat &cameraMatrix) const
{
	double *itC = cameraMatrix.ptr<double>(0, 0);
	for(size_t i = 0; i < 9; ++i, ++itC)
	{
		*itC = cameraInfo->K[i];
	}
}
 
/**
 * @ 将深度图像映射成彩色图
 * 
 */
void dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue)
{
cv::Mat tmp = cv::Mat(in.rows, in.cols, CV_8U);
const uint32_t maxInt = 255;

//  #pragma omp parallel for
for(int r = 0; r < in.rows; ++r)
{
	const uint16_t *itI = in.ptr<uint16_t>(r);
	uint8_t *itO = tmp.ptr<uint8_t>(r);

	for(int c = 0; c < in.cols; ++c, ++itI, ++itO)
	{
	*itO = (uint8_t)std::min((*itI * maxInt / maxValue), 255.0f);
	}
}

cv::applyColorMap(tmp, out, cv::COLORMAP_JET);
}
/**
 * @ 将深度图像叠加在彩色图像上
 * 
 */
void combine(const cv::Mat &inC, const cv::Mat &inD, cv::Mat &out)
{
out = cv::Mat(inC.rows, inC.cols, CV_8UC3);

//   #pragma omp parallel for
for(int r = 0; r < inC.rows; ++r)
{
	const cv::Vec3b
	*itC = inC.ptr<cv::Vec3b>(r),
	*itD = inD.ptr<cv::Vec3b>(r);
	cv::Vec3b *itO = out.ptr<cv::Vec3b>(r);

	for(int c = 0; c < inC.cols; ++c, ++itC, ++itD, ++itO)
	{
	itO->val[0] = (itC->val[0] + itD->val[0]) >> 1;
	itO->val[1] = (itC->val[1] + itD->val[1]) >> 1;
	itO->val[2] = (itC->val[2] + itD->val[2]) >> 1;
	}
}
}

  /*
  void generatePointCloud(const cv::Mat &depth, const cv::Mat &color, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud) const
  {
	  float cx = cameraMatrixColor.at<float>(0,2) ;
	  float cy = cameraMatrixColor.at<float>(1,2);
	  float fx = cameraMatrixColor.at<float>(0,0);
	  float fy = cameraMatrixColor.at<float>(1,1);
 
	  
        for ( int m=0; m<depth.rows; m+=3 )
		{
			for ( int n=0; n<depth.cols; n+=3 )
			{
				float d = depth.ptr<float>(m)[n];
				if (d < 0.01 || d>10)
					continue;
				pcl::PointXYZRGBA p;
				p.z = d;
				p.x = ( n - cx) * p.z / fx;
				p.y = ( m - cy) * p.z / fy;  
				
				p.b = color.ptr<uchar>(m)[n*3];
				p.g = color.ptr<uchar>(m)[n*3+1];
				p.r = color.ptr<uchar>(m)[n*3+2];
					
				cloud->points.push_back(p);
			}
		}
  }

  void saveCloudAndImages(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud, const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depthColored)
  {
    oss.str("");
    oss << "./" << std::setfill('0') << std::setw(4) << frame;
    const std::string baseName = oss.str();
    const std::string cloudName = baseName + "_cloud.pcd";
    const std::string colorName = baseName + "_color.jpg";
    const std::string depthName = baseName + "_depth.png";
    const std::string depthColoredName = baseName + "_depth_colored.png";

    OUT_INFO("saving cloud: " << cloudName);
    writer.writeBinary(cloudName, *cloud);
    OUT_INFO("saving color: " << colorName);
    cv::imwrite(colorName, color, params);
    OUT_INFO("saving depth: " << depthName);
    cv::imwrite(depthName, depth, params);
    OUT_INFO("saving depth: " << depthColoredName);
    cv::imwrite(depthColoredName, depthColored, params);
    OUT_INFO("saving complete!");
    ++frame;
  }

  void createLookup(size_t width, size_t height)
  {
        const float fx = 1.0f / cameraMatrixColor.at<double>(0, 0);
        const float fy = 1.0f / cameraMatrixColor.at<double>(1, 1);
        const float cx = cameraMatrixColor.at<double>(0, 2);
        const float cy = cameraMatrixColor.at<double>(1, 2);
        float *it;

        lookupY = cv::Mat(1, height, CV_32F);
        it = lookupY.ptr<float>();
        for(size_t r = 0; r < height; ++r, ++it)
        {
          *it = (r - cy) * fy;
        }

        lookupX = cv::Mat(1, width, CV_32F);
        it = lookupX.ptr<float>();
        for(size_t c = 0; c < width; ++c, ++it)
        {
          *it = (c - cx) * fx;
        }
  }
*/
void pointcloudpublish(pcl::PointCloud<pcl::PointXYZRGBA> inputMap )
 {
	   
	//pcl::PointCloud<pcl::PointXYZ> cloud; 	
	sensor_msgs::PointCloud2 output;   //声明的输出的点云的格式
	
	//pcl::toPCLPointCloud2(*p,p2); //转换到 PCLPointCloud2 类型
	//pcl_conversions::fromPCL(p2, output);    //转换到 ros 下的格式
  
	//pcl::io::loadPCDFile ("/home/crp/catkin_ws/test.pcd", cloud);  
	toROSMsg(inputMap,output);
	output.header.stamp=ros::Time::now();
	output.header.frame_id  ="world";
	pub_pointcloud.publish(output);
 }

};

int main(int argc, char **argv)
{
  
    std::string topicColor ,topicDepth ,cameraParamFile;
    
    ros::init(argc, argv, "orbslam_ros", ros::init_options::AnonymousName);

    if(!ros::ok())
    {
	    return 0;
    }
    

    if(ros::param::get("~topicColor" ,topicColor))
	    ;
    else
	    topicColor = "/kinect2/qhd/image_color_rect";

    if(ros::param::get("~topicDepth" ,topicDepth))
	    ;
    else
	    topicDepth = "/kinect2/qhd/image_depth_rect";
    if(ros::param::get("~cameraParamFile" ,cameraParamFile))
	    ;
    else
	cameraParamFile= "/home/crp/catkin_ws/src/orbslam2_ros/kinect2_qhd.yaml" ;
 
    string orbVoc = "/home/crp/catkin_ws/src/SLAM/orbslam2_pointcloud/ORBvoc.txt" ;
    
  
    
    bool orbmode = RGBD;
    bool orbshow = true;


    Receiver receiver(topicColor, topicDepth, cameraParamFile,orbVoc,orbmode, orbshow);

    cout<<"starting receiver..."<<endl;

    receiver.run();

    ros::shutdown();
    return 0;
}
